﻿#include "SerialPinch.h"
#include <QDebug>
#include <QFile>
#include <QVariantList>
#include <QVariantMap>
#include "JsonUtil.h"

#include "commander.h"
#include "commander_old.h"

SerialPinch::SerialPinch(int argc, char** argv, QObject* parent)
    : argc_(argc), argv_(argv), QObject(parent)
{
    m_com = new QSerialPort(this);
    connect(m_com, SIGNAL(readyRead()), this, SLOT(onReadReady()));

    m_nBuffIndex = 0;

    m_ros_spin_thread = std::thread{std::bind(&SerialPinch::rosSpinThread, this)};
}

SerialPinch::~SerialPinch()
{
    rclcpp::shutdown();
}

void SerialPinch::write(unsigned char* data, int len)
{
    QByteArray ba((char*)data, len);
    if (nullptr != m_com && m_com->isOpen()) {
        m_com->write(ba);
    }
}

void SerialPinch::setCom(const std::string& comName, int maxI, int cmderType)
{
    m_nMaxI = maxI;
    m_nCmderType = cmderType;
    qDebug() << "openCom: " << QString::fromStdString(comName);
    m_com->setPortName(QString::fromStdString(comName));
    m_com->setBaudRate(QSerialPort::Baud9600);
    m_com->setParity(QSerialPort::NoParity);
    m_com->setDataBits(QSerialPort::Data8);
    m_com->setStopBits(QSerialPort::OneStop);
    QMetaObject::invokeMethod(this, "openCom", Qt::QueuedConnection);
}

void SerialPinch::openCom()
{
    bool bOpen = m_com->open(QIODevice::ReadWrite);
    qDebug() << "openCom " << (bOpen ? "OK" : "NG");

    if (bOpen) {
        // quint8 data[8]{1, 2, 3, 4, 5, 6, 7, 8};
        // write(data, 8);

        m_cmder = new Commander();
        // connect(m_cmder, SIGNAL(sigJudge(QByteArray)), this, SLOT(onJudge(QByteArray)));

        m_cmderOld = &CommanderOld::instance();

        if (m_nCmderType == 1) {
            m_cmderOld->setSerialPort(m_com);
        }
    }
}

void SerialPinch::closeCom()
{
    m_com->close();
}

void SerialPinch::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

void SerialPinch::spin45()
{
    if (m_nCmderType == 2)  // new tool
    {
        QMetaObject::invokeMethod(this, "spin45_p", Qt::QueuedConnection);
    } else  // m_nCmderType == 1, old tool
    {
        QMetaObject::invokeMethod(this, "spin45_p_old", Qt::QueuedConnection);
    }
}

void SerialPinch::songkai()
{
    if (m_nCmderType == 2)  // new tool
    {
        QMetaObject::invokeMethod(this, "songkai_p", Qt::QueuedConnection);
    } else  // m_nCmderType == 1, old tool
    {
        QMetaObject::invokeMethod(this, "songkai_p_old", Qt::QueuedConnection);
    }
}

void SerialPinch::suojin180x9()
{
    if (m_nCmderType == 2)  // new tool
    {
        QMetaObject::invokeMethod(this, "suojin180x9_p", Qt::QueuedConnection);
    } else  // m_nCmderType == 1, old tool
    {
        QMetaObject::invokeMethod(this, "suojin180x9_p_old", Qt::QueuedConnection);
    }
}

void SerialPinch::callSpinBack45()
{
    if (m_nCmderType == 2)  // new tool
    {
        QMetaObject::invokeMethod(this, "spinBack45", Qt::QueuedConnection);
    } else  // m_nCmderType == 1, old tool
    {
        QMetaObject::invokeMethod(this, "spinBack45_old", Qt::QueuedConnection);
    }
}

void SerialPinch::callResetPos()
{
    QMetaObject::invokeMethod(this, "resetPos", Qt::QueuedConnection);
}

void SerialPinch::callStop()
{
    QMetaObject::invokeMethod(this, "stop", Qt::QueuedConnection);
}

void SerialPinch::spin45_p()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("spin45", arr);

    write(arr, 23);
}

void SerialPinch::songkai_p()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("songkai", arr, m_nMaxI);

    write(arr, 23);
}

void SerialPinch::suojin180x9_p()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("setI", arr, m_nMaxI);

    write(arr, 23);
}

void SerialPinch::spinBack45()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("spinBack45", arr);

    write(arr, 23);
}

void SerialPinch::stop()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("stop", arr);

    write(arr, 23);
}

void SerialPinch::resetPos()
{
    quint8 arr[23] = {0};
    int nCmdLen = m_cmder->getCmd("repos", arr);

    write(arr, 23);
}

void SerialPinch::spin45_p_old()
{
    CommanderOld::instance().setCurrent2(600);
    CommanderOld::instance().setVelocity2(15000);
    CommanderOld::instance().readPos2();
    CommanderOld::instance().readU2();
    int a = (CommanderOld::instance().getAbsPos2()) / 2 + 22 * 0.5f;
    CommanderOld::instance().toPos2(a);
}

void SerialPinch::songkai_p_old()
{
    CommanderOld::instance().setCurrent2(600);
    CommanderOld::instance().setVelocity2(15000);
    CommanderOld::instance().toPos2(0);
    CommanderOld::instance().readPos2();
    CommanderOld::instance().readU2();
}

void SerialPinch::suojin180x9_p_old()
{
    CommanderOld::instance().setCurrent2(800);
    CommanderOld::instance().setVelocity2(15000);
    CommanderOld::instance().toPos2(180 * 9 * 0.5f);
    CommanderOld::instance().readPos2();
    CommanderOld::instance().readU2();
}

void SerialPinch::spinBack45_old()
{
    CommanderOld::instance().setCurrent2(600);
    CommanderOld::instance().setVelocity2(15000);
    CommanderOld::instance().readPos2();
    CommanderOld::instance().readU2();
    int a = (CommanderOld::instance().getAbsPos2()) / 2 - 22 * 0.5f;
    CommanderOld::instance().toPos2(a);
}

void SerialPinch::onReadReady()
{
    if (m_nCmderType == 1) {
        return;
    }

    int nRead = m_com->read((char*)m_buff + m_nBuffIndex, BUFF_LEN - m_nBuffIndex);
    m_nBuffIndex += nRead;

    int nParsed = m_cmder->parseCmd(m_buff, m_nBuffIndex);
    memmove(m_buff, m_buff + nParsed, m_nBuffIndex - nParsed);
    m_nBuffIndex -= nParsed;
}

void SerialPinch::onJudge(const QByteArray& ba)
{
    if (nullptr != m_com && m_com->isOpen()) {
        m_com->write(ba);
    }
}
